#include "wheel_preintegration.h"

WheelPreIntegration::WheelPreIntegration(WheelOptions options) {
    options_ = options;
}
// t, px, py
void WheelPreIntegration::Integrate(const SE2& last_odometry, const SE2& current_odometry) {
    // preintegration
    SE2 odok = current_odometry - last_odometry;
    Eigen::Vector2d odork = odok.translation();
    Eigen::Matrix2d Phi_ik = dR_.matrix();
    dp_ += Phi_ik * odork;
    dR_ = dR_ * odok.so2();

    // std::cout << " Phi_ik =======: " << Phi_ik << std::endl;
    // std::cout << " odork =======: " << odork << std::endl;
    // std::cout << " wheel dp_0 =======: " << dp_.transpose() << std::endl;
    // std::cout << " wheel dR_0 =======: " << dR_.log() << std::endl;
 
    Eigen::Matrix3d Ak = Eigen::Matrix3d::Identity();
    Eigen::Matrix3d Bk = Eigen::Matrix3d::Identity();
    Ak.block<2,1>(1,0) = Phi_ik * Eigen::Vector2d(-odork[1], odork[0]);
    Bk.block<2,2>(1,1) = Phi_ik;

    Eigen::Matrix3d Sigma_vk = Eigen::Matrix3d::Identity();

    Sigma_vk(0,0) = options_.sigma_t * options_.sigma_t;
    Sigma_vk(1,1) = options_.sigma_x * options_.sigma_x;
    Sigma_vk(2,2) = options_.sigma_y * options_.sigma_y;

    cov_ =  Ak * cov_ * Ak.transpose() + Bk * Sigma_vk * Bk.transpose();
}

void WheelPreIntegration::Predict(State& state) {
    SE2 delta_pose = SE2(dR_, dp_);
    SE2 last_pose = SE2(state.R, state.p);
    SE2 update_pose = last_pose + delta_pose;
    state.R = update_pose.so2();
    state.p = update_pose.translation();
}

void WheelPreIntegration::Clear() {
    dR_ = SO2();
    dp_ = Eigen::Vector2d::Zero();  
    cov_ = Eigen::Matrix<double, 3, 3>::Zero(); 
}
